#include "camodocal.h"

#include <fstream>
#include <iostream>
#include <stdlib.h>

#include <sophus/se3.h>


struct CamPose
{
    double timestamp;
    Sophus::SE3 T;
};

void LoadCamPoseFromTxt(std::string filename, std::vector < CamPose >& allcampose )
{
    std::ifstream f;
    f.open(filename.c_str());

    if(!f.is_open())
    {
      std::cerr << " can't open cam pose file "<<std::endl;
      return;
    }

    while (!f.eof()) {

      std::string s;
      std::getline(f,s);

      if(! s.empty())
      {
        std::stringstream ss;
        ss << s;
        double  timestamp;
        ss >> timestamp;

        double x,y,z,q1,q2,q3,qw;
        ss >> x;
        ss >> y;
        ss >> z;
        ss >> q1;
        ss >> q2;
        ss >> q3;
        ss >> qw;

        Eigen::Quaterniond qua(qw, q1,q2,q3);
        Sophus::SE3 T(qua,Eigen::Vector3d(x,y,z));

        CamPose cp;
        cp.T = T;
        cp.timestamp = floor(timestamp/1000000);

        allcampose.push_back(cp);

        //list_T_w_r_.push_back(std::make_pair(timestamp,T));

      }
    }
}

void LoadPoseFromZhongXia(std::string filename, std::vector < CamPose >& allcampose )
{
    std::ifstream f;
    f.open(filename.c_str());

    if(!f.is_open())
    {
        std::cerr << " can't open cam pose file "<<std::endl;
        return;
    }

    while (!f.eof()) {

        std::string s;
        std::getline(f,s);

        if(! s.empty())
        {
            std::stringstream ss;
            ss << s;
            double  timestamp;

            char ch;
            ss >> timestamp;ss >> ch;

            double x,y,z,q1,q2,q3,qw;
            ss >> qw;ss >> ch;

            ss >> q1;ss >> ch;
            ss >> q2;ss >> ch;
            ss >> q3;ss >> ch;
            ss >> x; ss >> ch;
            ss >> y; ss >> ch;
            ss >> z;

            Eigen::Quaterniond qua(qw, q1,q2,q3);
            Sophus::SE3 T(qua,Eigen::Vector3d(x,y,z));

            CamPose cp;
            cp.T = T;
            cp.timestamp = floor(timestamp/1000000);

            allcampose.push_back(cp);

            //list_T_w_r_.push_back(std::make_pair(timestamp,T));

        }
    }
}

#define LASER2ODOM
void TestWithRealData()
{

    std::vector< CamPose > odompose;
    std::vector< CamPose > allcampose;

    LoadPoseFromZhongXia("/media/hyj/DISK_IMG/vo/encoder.txt",odompose);      // odom LoadPoseFromZhongXia
    LoadCamPoseFromTxt("/home/hyj/slam_project/vo_and_odocal/svo_edge/bin/campose_readface.txt",allcampose);

    bool initflag = true;
    Sophus::SE3 T_w_o_1, T_w_o_2;
    Sophus::SE3 T_w_c_1, T_w_c_2;

    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > quats_odo;
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tvecs_odo;
    std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > quats_cam;
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tvecs_cam;

    int posesize = allcampose.size();
    // 遍历所有图像关键帧pose
    for( int i = 10; i< posesize - 20; i = i+15 )  // allcampose.size()
    {
        CamPose cp;
        cp = allcampose[i];

        //单目相机完成初始化前，所有坐标都是在原点，舍弃
        if ( cp.T.translation().norm() < 1e-10) continue;

        if( initflag )
        {
            T_w_c_1 = cp.T;

            for (int j = 0; j < odompose.size(); ++j) {
                j = 0;
                if(cp.timestamp > (odompose[0].timestamp - 0.001))
                    odompose.erase(odompose.begin());
                else
                    break;
            }
            T_w_o_1 = odompose[0].T;

            initflag = false;
            continue;
        }

        T_w_c_2 = cp.T;
        for (int j = 0; j < odompose.size(); ++j) {
            j = 0;
            if(cp.timestamp > (odompose[0].timestamp - 0.001))
                odompose.erase(odompose.begin());
            else
                break;
        }
        T_w_o_2 = odompose[0].T;

        Sophus::SE3 T_o21 =T_w_o_2.inverse() * T_w_o_1 ;
        Sophus::SE3 T_c21 = T_w_c_2.inverse() * T_w_c_1;
        //Sophus::SE3 T_c21 =  T_w_c_1.inverse() * T_w_c_2;

        Sophus::SE3 T_o12_debug = T_o21.inverse() ;
        Sophus::SE3 T_c12_debug = T_c21.inverse() ;


        quats_odo.push_back(T_o21.unit_quaternion());
        tvecs_odo.push_back(T_o21.translation());

        quats_cam.push_back(T_c21.unit_quaternion());
        tvecs_cam.push_back(T_c21.translation());

        T_w_o_1 = T_w_o_2;
        T_w_c_1 = T_w_c_2;


    }

    Camodo::CamOdoCalibration coc;
    Eigen::Matrix4d Trc;
    std::cout<<"quats_odo size: "<<quats_odo.size()<<std::endl;
    coc.estimate(quats_odo,tvecs_odo,quats_cam,tvecs_cam,Trc);
    std::cout<<Trc<<std::endl;

}

int main(int argc, char** argv)
{

   //TestWithRandomData();
   //TestWithManMadeData();
   TestWithRealData();

  return 0;
}
